function [ out, auxOut ] = sini_pgs( specVelInc, angleInc, Dt, g, pos, CB0L, cst, beta_m_ext, DvScul_m_ext, DrScrl_m_ext )
%SINI_PGS 捷联惯性导航积分算法解算
%
% Input Arguments
% # specVelInc: m*3矩阵，高速解算周期内的比速度增量，单位m/s
% # angleInc: m*3矩阵，高速解算周期内的角增量，单位rad
% # Dt: 标量，IMU采样周期，单位s
% # g: 标量，外部给出的重力加速度，若小于0将使用内部重力模型，单位m/s^2
% # pos: 3*1向量，初始位置（纬度/单位rad、经度/单位rad、高度/单位m）
% # CB0L: 3*3矩阵，姿态矩阵0时刻量
% # cst: 常量结构体，参见sinistep_pgs函数说明
% # beta_m_ext, DvScul_m_ext, DrScrl_m_ext: m*3矩阵，外部给出的圆锥、划桨、涡卷效应补偿项，默认为空，具体参见sinistep_pgs函数说明
%
% Output Arguments
% # out: m*1结构向量，各周期内sinistep_pgs函数的输出
% # auxOut: m*1结构向量，各周期内sinistep_pgs函数的辅助输出
%
% Assumptions and Limitations
% # 本函数假设初始速度为0
% References:
% #《应用导航算法工程基础》“捷联惯性导航数值积分算法”

m = size(specVelInc, 1);
if nargin < 10
    DrScrl_m_ext = NaN(0, 3);
end
if nargin < 9
    DvScul_m_ext = NaN(0, 3);
end
if nargin < 8
    beta_m_ext = NaN(0, 3);
end

% 输入量
in_SINISPGS.specVelInc = specVelInc(1, :)';
in_SINISPGS.angleInc = angleInc(1, :)';
in_SINISPGS.g = g;
in_SINISPGS.hAltm = NaN;
in_SINISPGS.CNEExt = NaN(3);
in_SINISPGS.hExt = NaN;
in_SINISPGS.vNExt = NaN(3, 1);
in_SINISPGS.CBLExt = NaN(3);
in_SINISPGS.qBLExt = NaN(1, 4);
in_SINISPGS.beta_m_ext = NaN(3, 1);
in_SINISPGS.DvScul_m_ext = NaN(3, 1);
in_SINISPGS.DrScrl_m_ext = NaN(3, 1);

% 配置
cfg_SINISPGS.NSInLS = 1;
cfg_SINISPGS.HSInNS = 1;
cfg_SINISPGS.useDCMInAttCalc = false;
cfg_SINISPGS.useNormOrthoInAttCalc = true;
cfg_SINISPGS.useHiResPosNSCal = true;
if g < 0
    cfg_SINISPGS.useExtGravity = false;
else
    cfg_SINISPGS.useExtGravity = true;
end
cfg_SINISPGS.navCoordType = NavCoordType.GEO;

% 参量
par_SINISPGS.Tl = Dt;
par_SINISPGS.C = zeros(1, 3);

% 初始条件
initCon_SINISPGS.L_n = pos(1, 1);
initCon_SINISPGS.L_n1 = pos(1, 1);
initCon_SINISPGS.lambda_n = pos(2, 1);
initCon_SINISPGS.lambda_n1 = pos(2, 1);
initCon_SINISPGS.h_n = pos(3, 1);
initCon_SINISPGS.h_n1 = pos(3, 1);
initCon_SINISPGS.vN_m = zeros(3, 1);
initCon_SINISPGS.vN_m1 = zeros(3, 1);
initCon_SINISPGS.vN_n1 = zeros(3, 1);
initCon_SINISPGS.CBL = CB0L;
initCon_SINISPGS.specVelInc_lx = specVelInc(1, :)'; % 0时刻之前的比速度增量
initCon_SINISPGS.angleInc_lx = angleInc(1, :)';

% 运行
[out, auxOut] = sinistep_pgs(int8(-1), in_SINISPGS, cfg_SINISPGS, par_SINISPGS, initCon_SINISPGS, cst);
out = repmat(out, m+1, 1);
auxOut = repmat(auxOut, m+1, 1);
hWB = waitbar_step(0, 0.01, 'Running strapdown inertial navigation integration.');
for i=0:m
    waitbar_step(i/m, 0.01, hWB);
    if i == 0
        mode = int8(0);
        dataInd = 1;
    else
        mode = int8(1);
        dataInd = i;
    end
    in_SINISPGS.specVelInc = specVelInc(dataInd, :)';
    in_SINISPGS.angleInc = angleInc(dataInd, :)';
    if ~isempty(beta_m_ext)
        in_SINISPGS.beta_m_ext = beta_m_ext(dataInd, :)';
    end
    if ~isempty(DvScul_m_ext)
        in_SINISPGS.DvScul_m_ext = DvScul_m_ext(dataInd, :)';
    end
    if ~isempty(DrScrl_m_ext)
        in_SINISPGS.DrScrl_m_ext = DrScrl_m_ext(dataInd, :)';
    end
    [out(i+1), auxOut(i+1)] = sinistep_pgs(mode, in_SINISPGS, cfg_SINISPGS, par_SINISPGS, initCon_SINISPGS, cst);
end
close(hWB);
end